package trb.complexshield.bool;

import no.hrp.hvrc.rad.calc.intersect.Segment;
import no.hrp.hvrc.utils.j3d.BBox;
import no.hrp.hvrc.utils.j3d.Mat4;

import javax.media.j3d.Transform3D;
import javax.vecmath.Point3f;
import javax.vecmath.Tuple3f;
import javax.vecmath.Vector3f;

/**
 * An axis aligned box.
 *
 * @author tomrbryn
 * @author RuneV
 */
public class IntersectableBox implements SegmentIntersectable {
    // elements 0-2 is lower x, y, z. elements 3-5 is upper x, y, z
    private final float[] minMax = new float[6];

    // the shields local to world matrix
    private final Transform3D local2World = new Transform3D();

    // the world to local space transformation matrix
    private final Transform3D world2local = new Transform3D();

    // temp var, the transformed line
    private final Segment l = new Segment(new Vector3f(), new Vector3f(0, 1, 0));

    private BBox aabb;

    private float[] w2l = new float[16];

    /**
     * Creates a box from the specified lower and upper corner of the box
     * @param lower the lower corner
     * @param upper the upper corner
     * @param t3d the local to world matrix
     */
    public IntersectableBox(Tuple3f lower, Tuple3f upper, Transform3D t3d) {
        minMax[0] = lower.x;
        minMax[1] = lower.y;
        minMax[2] = lower.z;
        minMax[3] = upper.x;
        minMax[4] = upper.y;
        minMax[5] = upper.z;
        world2local.invert(t3d);
        local2World.set(t3d);
        aabb = new BBox(lower, upper);
        aabb.transform(local2World);
        world2local.get(w2l);
    }

    public float getBoxDepth(){
        return Math.abs(minMax[5]-minMax[2]);
    }

    public float getBoxLength(){
        return Math.abs(minMax[3]-minMax[0]);
    }

    public float[] getMinMax() {
        return minMax;
    }

    public Mat4 getLocalToWorld() {
        return new Mat4(local2World);
    }

    public Mat4 getWorldToLocal() {
        return new Mat4(world2local);
    }

    @Override
    public boolean intersects(Segment line, float[] tMinMaxOut) {
        return getLengthInside(line, minMax, w2l, tMinMaxOut);
    }

    public static boolean getLengthInside(Segment line, float[] minMax, float[] w2l, float[] tMinMaxOut) {
        Point3f origin = line.originRef();
        Point3f target = line.targetRef();
        return getLengthInside(origin.x, origin.y, origin.z, target.x, target.y, target.z, minMax, 0, w2l, 0, tMinMaxOut);
    }

    public static boolean getLengthInside(float originx, float originy, float originz
            , float targetx, float targety, float targetz
            , float[] minMax, int minMaxOff, float[] w2l, int w2lOff, float[] tMinMaxOut) {
        // transform start point into box local space
        float x1 = w2l[w2lOff + 0] * originx + w2l[w2lOff + 1] * originy + w2l[w2lOff + 2]  * originz + w2l[w2lOff + 3];
        float y1 = w2l[w2lOff + 4] * originx + w2l[w2lOff + 5] * originy + w2l[w2lOff + 6]  * originz + w2l[w2lOff + 7];
        float z1 = w2l[w2lOff + 8] * originx + w2l[w2lOff + 9] * originy + w2l[w2lOff + 10] * originz + w2l[w2lOff + 11];
        float x2 = w2l[w2lOff + 0] * targetx + w2l[w2lOff + 1] * targety + w2l[w2lOff + 2]  * targetz + w2l[w2lOff + 3];
        float y2 = w2l[w2lOff + 4] * targetx + w2l[w2lOff + 5] * targety + w2l[w2lOff + 6]  * targetz + w2l[w2lOff + 7];
        float z2 = w2l[w2lOff + 8] * targetx + w2l[w2lOff + 9] * targety + w2l[w2lOff + 10] * targetz + w2l[w2lOff + 11];

        // create normalised direction vector
        float directionX = x2 - x1;
        float directionY = y2 - y1;
        float directionZ = z2 - z1;
        float directionLength = (float) Math.sqrt(directionX * directionX + directionY * directionY + directionZ * directionZ);
        float ooDirectionLength = 1.0f / directionLength;
        directionX *= ooDirectionLength;
        directionY *= ooDirectionLength;
        directionZ *= ooDirectionLength;

        float invDirectionX = 1.0f / directionX;
        float invDirectionY = 1.0f / directionY;
        float invDirectionZ = 1.0f / directionZ;

        int signX = (invDirectionX < 0 ? 1 : 0);
        int signY = (invDirectionY < 0 ? 1 : 0);
        int signZ = (invDirectionZ < 0 ? 1 : 0);

        // algorithm taken from: http://www.cs.utah.edu/~awilliam/box/
        // is accurate and optimized
        float tmin  = (minMax[minMaxOff +      signX  * 3 + 0] - x1) * invDirectionX;
        float tmax  = (minMax[minMaxOff + (1 - signX) * 3 + 0] - x1) * invDirectionX;
        float tymin = (minMax[minMaxOff +      signY  * 3 + 1] - y1) * invDirectionY;
        float tymax = (minMax[minMaxOff + (1 - signY) * 3 + 1] - y1) * invDirectionY;
        if ((tmin > tymax) || (tymin > tmax)) {
            return false;
        }
        if (tmin < tymin) {
            tmin = tymin;
        }
        if (tmax > tymax) {
            tmax = tymax;
        }
        float tzmin = (minMax[minMaxOff +      signZ  * 3 + 2] - z1) * invDirectionZ;
        float tzmax = (minMax[minMaxOff + (1 - signZ) * 3 + 2] - z1) * invDirectionZ;
        if ((tmin > tzmax) || (tzmin > tmax)) {
            return false;
        }
        if (tmin < tzmin) {
            tmin = tzmin;
        }
        if (tmax > tzmax) {
            tmax = tzmax;
        }
        if (tmin < 0) {
            tmin = 0;
        }
        if (tmax > directionLength) {
            tmax = directionLength;
        }
        tMinMaxOut[0] = tmin * ooDirectionLength;
        tMinMaxOut[1] = tmax * ooDirectionLength;
        return tmax > tmin;
    }

    public BBox getAABB() {
        return aabb;
    }
}